#pragma config(Hubs,  S1, HTMotor,  HTServo,  HTMotor,  none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     armtouch,       sensorTouch)
#pragma config(Sensor, S3,     Light,          sensorLightActive)
#pragma config(Sensor, S4,     Touch,          sensorTouch)
#pragma config(Motor,  motorA,          harvesterL,    tmotorNormal, openLoop)
#pragma config(Motor,  motorB,          harvesterR,    tmotorNormal, openLoop, reversed)
#pragma config(Motor,  motorC,          ,              tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_1,     motorR,        tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     motorL,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     elbow,         tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C3_2,     harvesterlift, tmotorNone, openLoop)
#pragma config(Servo,  srvo_S1_C2_1,    leftservo,            tServoContinuousRotation)
#pragma config(Servo,  srvo_S1_C2_2,    rightservo,           tServoContinuousRotation)
#pragma config(Servo,  srvo_S1_C2_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

 /////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                           Tele-Operation Mode Code Template
//
// This file contains a template for simplified creation of an tele-op program for an FTC
// competition.
//
// You need to customize two functions with code unique to your specific robot.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.
#define min(a,b) (a<b)?a:b
#define max(a,b) (a>b)?a:b


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                    initializeRobot
//
// Prior to the start of tele-op mode, you may want to perform some initialization on your robot
// and the variables within your program.
//
// In most cases, you may not have to add any code to this function and it will remain "empty".
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

void initializeRobot()
{
  // Place code here to sinitialize servos to starting positions.
  // Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.

  return;
}


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                         Main Task
//
// The following is the main code for the tele-op robot operation. Customize as appropriate for
// your specific robot.
//
// Game controller / joystick information is sent periodically (about every 50 milliseconds) from
// the FMS (Field Management System) to the robot. Most tele-op programs will follow the following
// logic:
//   1. Loop forever repeating the following actions:
//   2. Get the latest game controller / joystick settings that have been received from the PC.
//   3. Perform appropriate actions based on the joystick + buttons settings. This is usually a
//      simple action:
//      *  Joystick values are usually directly translated into power levels for a motor or
//         position of a servo.
//      *  Buttons are usually used to start/stop a motor or cause a servo to move to a specific
//         position.
//   4. Repeat the loop.
//
// Your program needs to continuously loop because you need to continuously respond to changes in
// the game controller settings.
//
// At the end of the tele-op period, the FMS will autonmatically abort (stop) execution of the program.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

task main()
{
  initializeRobot();

  waitForStart();   // wait for start of tele-op phase
  int threshold = 20;             /* Int 'threshold' will allow us to ignore low       */
                                  /* readings that keep our robot in perpetual motion. */
//  servoChangeRate[wrist] = 20;
  float scale = 0.5; // 0.3; //1.0; // 0.5;
  while (true)
  {
	  ///////////////////////////////////////////////////////////
	  ///////////////////////////////////////////////////////////
	  ////                                                   ////
	  ////      Add your robot specific tele-op code here.   ////
	  ////                                                   ////
	  ///////////////////////////////////////////////////////////
	  ///////////////////////////////////////////////////////////

    // Insert code to have servos and motors respond to joystick and button values.

    // Look in the ROBOTC samples folder for programs that may be similar to what you want to perform.
    // You may be able to find "snippets" of code that are similar to the functions that you want to
    // perform.

      getJoystickSettings(joystick);

      if (joy1Btn (5)== 1 ){
        scale = 0.8;
    }
      else if (joy1Btn (7)== 1 ){
         scale = 0.3;
      }
      else {
        //scale = 0.5;
      }
     if (joy2Btn(7) == 1 ) {
       motor[elbow] = -20;
     } else if (joy2Btn(5) == 1 ) {
     motor[elbow] = 20;
   } else {
   motor[elbow] = 0;
 }
 getJoystickSettings(joystick);

     if (joy2Btn(1) == 1 ) {
       motor[harvesterL] = -100;
     } else if (joy2Btn(3) == 1 ) {
     motor[harvesterL] = 100;
   } else {
   motor[harvesterL] = 0;
 }
 getJoystickSettings(joystick);

     if (joy2Btn(2) == 1 ) {
       motor[harvesterlift] = -20;
     } else if (joy2Btn(4) == 1 ) {
     motor[harvesterlift] = 20;
   } else {
   motor[harvesterlift] = 0;
 }
getJoystickSettings(joystick);

     if (joy2Btn(1) == 1 ) {
       motor[harvesterR]=100;
     } else if (joy2Btn(3) == 1 ) {
     motor[harvesterR] = -100;
   } else {
   motor[harvesterR] = 0;

}

getJoystickSettings(joystick);

     if (joy2Btn(6) == 1 ) {
 //      servo [wrist]=255;
     } else if (joy2Btn(8) == 1 ) {
 //    servo [wrist] = -0;


}

    if(abs(joystick.joy1_y2) > threshold)   // If the right analog stick's Y-axis readings are either above or below the threshold:
    {
      nxtDisplayTextLine(2,"y2=%d",joystick.joy1_y2);
      motor[motorL] = scale * joystick.joy1_y2;         // Motor B is assigned a power level equal to the right analog stick's Y-axis reading.
    }
    else                                    // Else if the readings are within the threshold:
    {
      motor[motorL] = 0;                        // Motor B is stopped with a power level of 0.
    }


    if(abs(joystick.joy1_y1) > threshold)   // If the left analog stick's Y-axis readings are either above or below the threshold:
    {
      nxtDisplayTextLine(1,"y1=%d",joystick.joy1_y1);
      motor[motorR] = scale * joystick.joy1_y1;         // Motor C is assigned a power level equal to the left analog stick's Y-axis reading.
    }
    else                                    // Else if the readings are within the threshold:
    {
      motor[motorR] = 0;                        // Motor C is stopped with a power level of 0.
    }

        if(joystick.joy2_y2 > 20)
    {
      servo[leftservo] = 255;
      servo[rightservo] = 0;
    }
    else if (joystick.joy2_y2 < -20)
    {
      servo[leftservo] = 0;
      servo[rightservo] = 255;

    }
    else
    {
      servo[leftservo] = 127;
      servo[rightservo] = 127;
    }

      // wait1Msec(300);

  }
}
